#include "boundingbox.h"

// Included files for OpenGL Rendering
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif

#include "cloth.h"
#include "argparser.h"
#include <fstream>
#include "vectors.h"

using namespace std;

// ================================================================================
// some helper drawing functions

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);
void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction);
Vec3f getNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);

// ================================================================================
// ================================================================================

Cloth::Cloth(ArgParser *_args) {
  args =_args;

  // open the file
  ifstream istr(args->cloth_file.c_str());
  assert (istr != NULL);
  string token;

  // read in the simulation parameters
  istr >> token >> k_structural; assert (token == "k_structural");  // (units == N/m)  (N = kg*m/s^2)
  istr >> token >> k_shear; assert (token == "k_shear");
  istr >> token >> k_bend; assert (token == "k_bend");
  istr >> token >> damping; assert (token == "damping");
  // NOTE: correction factor == .1, means springs shouldn't stretch more than 10%
  //       correction factor == 100, means don't do any correction
  istr >> token >> provot_structural_correction; assert (token == "provot_structural_correction");
  istr >> token >> provot_shear_correction; assert (token == "provot_shear_correction");

  // the cloth dimensions
  istr >> token >> nx >> ny; // (units == meters)
  assert (token == "m");
  assert (nx >= 2 && ny >= 2);

  // the corners of the cloth
  Vec3f a,b,c,d;
  istr >> token >> a; assert (token == "p");
  istr >> token >> b; assert (token == "p");
  istr >> token >> c; assert (token == "p");
  istr >> token >> d; assert (token == "p");

  // fabric weight  (units == kg/m^2)
  // denim ~300 g/m^2
  // silk ~70 g/m^2
  float fabric_weight;
  istr >> token >> fabric_weight; assert (token == "fabric_weight");
  float area = AreaOfTriangle(a,b,c) + AreaOfTriangle(a,c,d);

  // create the particles
  particles = new ClothParticle[nx*ny];
  float mass = area*fabric_weight / float(nx*ny);
  for (int i = 0; i < nx; i++) {
    float x = i/float(nx-1);
    Vec3f ab = (1-x)*a + x*b;
    Vec3f dc = (1-x)*d + x*c;
    for (int j = 0; j < ny; j++) {
      float y = j/float(ny-1);
      ClothParticle &p = getParticle(i,j);
      Vec3f abdc = (1-y)*ab + y*dc;
      p.setOriginalPosition(abdc);
      p.setPosition(abdc);
      p.setVelocity(Vec3f(0,0,0));
      p.setMass(mass);
      p.setFixed(false);
    }
  }

  // the fixed particles
  while (istr >> token) {
    assert (token == "f");
    int i,j;
    float x,y,z;
    istr >> i >> j >> x >> y >> z;
    ClothParticle &p = getParticle(i,j);
    p.setPosition(Vec3f(x,y,z));
    p.setFixed(true);
  }
  
  //Set spring relationships
  //
	for(int ii=0; ii<nx; ii++)
	{
		for(int jj=0; jj<ny; jj++)
		{
			//cout<<ii<<" "<<jj<<":"<<endl;
			if(ii+1<nx) //struct -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj);
				p.struc.push_back(make_pair(ii+1,jj));//p2);
				p2.struc.push_back(make_pair(ii,jj));//p);
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(jj+1<ny) //struct |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+1);
				p.struc.push_back(make_pair(ii,jj+1));//p2);
				p2.struc.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if( (jj+1<ny) && (ii+1<nx) ) //shear 
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj+1);
				p.shear.push_back(make_pair(ii+1,jj+1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if( (jj-1>=0) && (ii+1<nx) ) //shear /
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj-1);
				p.shear.push_back(make_pair(ii+1,jj-1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if(jj+2<ny) //bend |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+2);
				p.bend.push_back(make_pair(ii,jj+2));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(ii+2<nx) //bend -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+2,jj);
				p.bend.push_back(make_pair(ii+2,jj));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
		}
	}
	
	

	
  computeBoundingBox();
}

// ================================================================================

void Cloth::computeBoundingBox() {
  box = BoundingBox(getParticle(0,0).getPosition());
  for (int i = 0; i < nx; i++) {
    for (int j = 0; j < ny; j++) {
      box.Extend(getParticle(i,j).getPosition());
    }
  }
}

// ================================================================================
// ================================================================================
// ================================================================================

void Cloth::Paint() const 
{

  // =====================================================================================
  // render the bounding box
  // =====================================================================================
  if (args->bounding_box) 
    box.Paint();

  // =====================================================================================
  // render the particles
  // =====================================================================================

  if (args->particles) {
    glDisable(GL_LIGHTING);
    glPointSize(3);
    glBegin(GL_POINTS);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
	const ClothParticle &p = getParticle(i,j);
	const Vec3f &pos = p.getPosition();
	bool fixed = p.isFixed();
	if (fixed) 
	  glColor3f(0,1,0);
	else
	  glColor3f(0,0,0);	
	glVertex3f(pos.x(),pos.y(),pos.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the velocity at each particle
  // =====================================================================================

  if (args->velocity) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
        const Vec3f &pos = p.getPosition();
        const Vec3f &vel = p.getVelocity();
        Vec3f v = pos + vel;
        glVertex3f(pos.x(),pos.y(),pos.z());
        glVertex3f(v.x(),v.y(),v.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the force at each particle
  // =====================================================================================

  if (args->force) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    for (int i = 0; i < nx; i++) 
	{
      for (int j = 0; j < ny; j++) 
	  {
        const ClothParticle &p = getParticle(i,j);
		const Vec3f &pos = p.getPosition();
		Vec3f struc_force=Vec3f(0,0,0);
		Vec3f shear_force=Vec3f(0,0,0);
		Vec3f bend_force=Vec3f(0,0,0);
		for(int k=0; k<p.struc.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
			
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_structural;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}
		for(int k=0; k<p.shear.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );//-a; //Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			//s_pos=s_pos*k_shear;//*p.getMass();
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_shear;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}
		for(int k=0; k<p.bend.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );//-a; //Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			//s_pos=s_pos*k_bend;//*p.getMass();
			
			Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
			float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
			
			float diff=(dist-dist2);
			
			Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
			unit_vec=unit_vec*diff;
			s_pos=unit_vec*k_bend;
			
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			
		}

		Vec3f gravity_force=args->gravity*p.getMass();
		glVertex3f(pos.x(),pos.y(),pos.z());      
		glVertex3f(pos.x()+gravity_force.x(),pos.y()+gravity_force.y(),pos.z()+gravity_force.z());
	
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the cloth surface
  // =====================================================================================

  if (args->surface) {
    glColor3f(1,1,1);
    glBegin(GL_QUADS);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
	const Vec3f &a = getParticle(i,j).getPosition();
	const Vec3f &b = getParticle(i,j+1).getPosition();
	const Vec3f &c = getParticle(i+1,j+1).getPosition();
	const Vec3f &d = getParticle(i+1,j).getPosition();
	insertNormal(a,b,c,d);
	glVertex3f(a.x(),a.y(),a.z());
	glVertex3f(b.x(),b.y(),b.z());
	glVertex3f(c.x(),c.y(),c.z());
	glVertex3f(d.x(),d.y(),d.z());
      }
    }
    glEnd();
  }

  // =====================================================================================
  // render the wireframe cloth (the structural and shear springs)
  // =====================================================================================

  if (args->wireframe) {
    glDisable(GL_LIGHTING);
    glLineWidth(1);
    glBegin(GL_LINES);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
        const Vec3f &a_o = getParticle(i,j).getOriginalPosition();
        const Vec3f &b_o = getParticle(i,j+1).getOriginalPosition();
        const Vec3f &c_o = getParticle(i+1,j+1).getOriginalPosition();
        const Vec3f &d_o = getParticle(i+1,j).getOriginalPosition();        
        const Vec3f &a = getParticle(i,j).getPosition();
        const Vec3f &b = getParticle(i,j+1).getPosition();
        const Vec3f &c = getParticle(i+1,j+1).getPosition();
        const Vec3f &d = getParticle(i+1,j).getPosition();        
        DrawSpring(a_o,b_o,a,b, provot_structural_correction);
        DrawSpring(a_o,c_o,a,c, provot_shear_correction);
        DrawSpring(a_o,d_o,a,d, provot_structural_correction);
        DrawSpring(b_o,c_o,b,c, provot_structural_correction);
        DrawSpring(b_o,d_o,b,d, provot_shear_correction);
        DrawSpring(c_o,d_o,c,d, provot_structural_correction);
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  }
}

// ================================================================================
// ================================================================================

void Cloth::Animate() 
{
	bool pull_out=true; //Check if the timestep needs to be adjusted
	while(pull_out) //do this until an appropriate timestep has been found
	{
		pull_out=false;
		for (int i = 0; i < nx; i++) //go through each particle
		{
			for (int j = 0; j < ny; j++) 
			{
				ClothParticle &p = getParticle(i,j);
				Vec3f pos=p.getPosition();
				
				Vec3f struc_force=Vec3f(0,0,0); 
				Vec3f shear_force=Vec3f(0,0,0);
				Vec3f bend_force=Vec3f(0,0,0);
				
				for(int k=0; k<p.struc.size(); k++) //go through all struc spring connections
				{
					ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
					Vec3f a=p2.getPosition();
					Vec3f b=p.getPosition();
					Vec3f c=p2.getOriginalPosition();
					Vec3f d=p.getOriginalPosition(); 
					
					//find distance between the original and current position
					Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
					Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
					float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
					float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
					
					//find the force and check for provot correction
					Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
					float check=abs(dist/dist2);  
					Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
					struc_force=struc_force+internal; //add spring force to total structural spring force
					
					//adjust for provot in fixed point case
					if( provot_structural_correction*dist2 > abs(dist-dist2) && p2.isFixed()==true)  //( (check > (provot_structural_correction+1)) && p2.isFixed()==true )
					{
						Vec3f spot_p=b;
						Vec3f spot_p2=a;
						
						int count=2;
						Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.005));
						dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
						check=abs(dist/dist2);
						
						while( (check > (provot_structural_correction+1) ) && count<5)
						{
							n_loc=p.getPosition() + (unit_vec*(dist*.005*count));
							dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
							check=abs(dist/dist2);
							//count++;
						}
						p.setPosition(n_loc);	
					}
					//adjust for provot in non fixed point case
					else if( provot_structural_correction*dist2 > abs(dist-dist2) && p.isFixed()==false)
					{
						Vec3f spot_p=b;
						Vec3f spot_p2=a;
						Vec3f s_pos2=( Vec3f( b.x()-a.x(), b.y()-a.y(), b.z()-a.z() ) );
						Vec3f unit_vec2=Vec3f( s_pos2.x()/dist, s_pos2.y()/dist, s_pos2.z()/dist );
						
						int count=2;
						Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.0005));
						Vec3f n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005));
						
						dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
						check=abs(dist/dist2);
						
						while( (check > (provot_structural_correction+1) ) && count<5)
						{
							n_loc=p.getPosition() + (unit_vec*(dist*.0005*count));
							n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005*count));
							dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
							check=abs(dist/dist2);
							count++;
						}
						p.setPosition(n_loc);
					}		
				}
				for(int k=0; k<p.shear.size(); k++) //go through all shear spring connections, same methods as with structural
				{
					ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
					Vec3f a=p2.getPosition();
					Vec3f b=p.getPosition();
					Vec3f c=p2.getOriginalPosition();
					Vec3f d=p.getOriginalPosition();
					Vec3f s_pos=( Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) );
					Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
					
					float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
					float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
					
					Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
					float check=abs(dist/dist2);
					Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
					shear_force=shear_force+internal;		
					
					if( provot_shear_correction*dist2 > abs(dist-dist2) && p2.isFixed()==true)      
					{
						Vec3f spot_p=b;
						Vec3f spot_p2=a;
						
						int count=2;
						Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.005));
						dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
						check=abs(dist/dist2);
						
						while( (check > (provot_structural_correction+1) ) && count<5)
						{
							n_loc=p.getPosition() + (unit_vec*(dist*.005*count));
							dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
							check=abs(dist/dist2);
							//count++;
						}
						p.setPosition(n_loc);
						
					}
					else if( provot_shear_correction*dist2 > abs(dist-dist2) && p.isFixed()==false)
					{
						Vec3f spot_p=b;
						Vec3f spot_p2=a;
						Vec3f s_pos2=( Vec3f( b.x()-a.x(), b.y()-a.y(), b.z()-a.z() ) );
						Vec3f unit_vec2=Vec3f( s_pos2.x()/dist, s_pos2.y()/dist, s_pos2.z()/dist );
						
						int count=2;
						Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.0005));
						Vec3f n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005));
						
						dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
						check=abs(dist/dist2);
						
						while( (check > (provot_structural_correction+1) ) && count<5)
						{
							n_loc=p.getPosition() + (unit_vec*(dist*.0005*count));
							n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005*count));
							dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
							check=abs(dist/dist2);
							count++;
						}
						p.setPosition(n_loc); 
					}		
				}
				for(int k=0; k<p.bend.size(); k++) //find bend force
				{
					ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
					Vec3f a=p2.getPosition();
					Vec3f b=p.getPosition();
					Vec3f c=p2.getOriginalPosition();
					Vec3f d=p.getOriginalPosition();
					Vec3f s_pos=( Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) );
					Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
					
					float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) ); //current length
					float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) ); //original length
					
					Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
					Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
					bend_force=bend_force+internal;
				}
				
				if(p.isFixed()==false && pull_out==false) //find forces -> acceleration -> new position
				{
					Vec3f gravity_force=args->gravity*p.getMass();
					Vec3f accel = struc_force+shear_force+bend_force+gravity_force;
					
					accel=Vec3f(accel.x()/p.getMass(),accel.y()/p.getMass(),accel.z()/p.getMass());
					
					p.setAcceleration(accel);//*args->timestep);
					
					Vec3f Vi=p.getVelocity();
					Vec3f Vf=p.getVelocity()+ ( (p.getAcceleration()) * args->timestep);
					Vf=Vf*(1-damping);
					Vec3f Vif=Vi+Vf;
					
					Vec3f n_pos=(p.getPosition() + Vf*args->timestep);
					Vec3f pos=p.getPosition();
					
					float dist= sqrt( (n_pos.x()-pos.x())*(n_pos.x()-pos.x()) + (n_pos.y()-pos.y())*(n_pos.y()-pos.y()) + (n_pos.z()-pos.z())*(n_pos.z()-pos.z()) ); 
					float length= sqrt( (pos.x()*pos.x()) + (pos.y()*pos.y()) + (pos.z()*pos.z()) );
					if( dist > length*2 ) //IF THAT NEW POSITION IS TWICE THE DISTANCE OF THE LENGTH OF THE ORIGINAL POSITION, TURN DOWN THE TIMESTEP
					{
						args->timestep=args->timestep/2;
						pull_out=true;
					}
				}
			
			}
		}
	}

	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	
	//Now, move the cloth particles appropriately
	//Same Methods as above
	for (int i = 0; i < nx; i++) 
	{
		//if(pull_out==true) break;
		for (int j = 0; j < ny; j++) 
		{
			//if(pull_out==true) break;
			ClothParticle &p = getParticle(i,j);
			Vec3f pos=p.getPosition();
			
			Vec3f struc_force=Vec3f(0,0,0);
			Vec3f shear_force=Vec3f(0,0,0);
			Vec3f bend_force=Vec3f(0,0,0);
			
			for(int k=0; k<p.struc.size(); k++)
			{
				ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				
				Vec3f s_pos=Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
				Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
				
				Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
				float check=abs(dist/dist2);  
				Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
				struc_force=struc_force+internal;//+s_pos;
				
				if( provot_structural_correction*dist2 > abs(dist-dist2) && p2.isFixed()==true)  //( (check > (provot_structural_correction+1)) && p2.isFixed()==true )
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.005));
					dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.005*count));
						dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
						check=abs(dist/dist2);
						//count++;
					}
					p.setPosition(n_loc);
				}
				else if( provot_structural_correction*dist2 > abs(dist-dist2) && p.isFixed()==false)
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					Vec3f s_pos2=( Vec3f( b.x()-a.x(), b.y()-a.y(), b.z()-a.z() ) );
					Vec3f unit_vec2=Vec3f( s_pos2.x()/dist, s_pos2.y()/dist, s_pos2.z()/dist );
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.0005));
					Vec3f n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005));
					
					dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.0005*count));
						n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005*count));
						dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
						check=abs(dist/dist2);
						count++;
					}
					p.setPosition(n_loc);
				}	
				
			}
			for(int k=0; k<p.shear.size(); k++)
			{
				ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				Vec3f s_pos=( Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) );
				Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
				
				Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
				float check=abs(dist/dist2);
				Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
				shear_force=shear_force+internal;		
				
				if( provot_shear_correction*dist2 > abs(dist-dist2) && p2.isFixed()==true)      //( (check > (provot_shear_correction+1)) && p2.isFixed()==true && p.isFixed()==false)
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.005));
					dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.005*count));
						dist= sqrt( (a.x()-n_loc.x())*(a.x()-n_loc.x()) + (a.y()-n_loc.y())*(a.y()-n_loc.y()) + (a.z()-n_loc.z())*(a.z()-n_loc.z()) );
						check=abs(dist/dist2);
						//count++;
					}
					p.setPosition(n_loc);
					
				}
				else if( provot_shear_correction*dist2 > abs(dist-dist2) && p.isFixed()==false)
				{
					Vec3f spot_p=b;
					Vec3f spot_p2=a;
					Vec3f s_pos2=( Vec3f( b.x()-a.x(), b.y()-a.y(), b.z()-a.z() ) );
					Vec3f unit_vec2=Vec3f( s_pos2.x()/dist, s_pos2.y()/dist, s_pos2.z()/dist );
					
					int count=2;
					Vec3f n_loc=p.getPosition() + (unit_vec*(dist*.0005));
					Vec3f n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005));
					
					dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
					check=abs(dist/dist2);
					
					while( (check > (provot_structural_correction+1) ) && count<5)
					{
						n_loc=p.getPosition() + (unit_vec*(dist*.0005*count));
						n_loc2=p2.getPosition() + (unit_vec2*(dist*.0005*count));
						dist= sqrt( (n_loc2.x()-n_loc.x())*(n_loc2.x()-n_loc.x()) + (n_loc2.y()-n_loc.y())*(n_loc2.y()-n_loc.y()) + (n_loc2.z()-n_loc.z())*(n_loc2.z()-n_loc.z()) );
						check=abs(dist/dist2);
						count++;
					}
					p.setPosition(n_loc);
					 
				}	
				//if( check > 2 )
				//{
					//args->timestep=args->timestep/2;
					//pull_out=true;
					//break;
				//}
			}
			for(int k=0; k<p.bend.size(); k++)
			{
				ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				Vec3f s_pos=( Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) );
				Vec3f d_pos=Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) ); //current length
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) ); //original length
				
				Vec3f unit_vec=Vec3f( s_pos.x()/dist, s_pos.y()/dist, s_pos.z()/dist );
				Vec3f internal=k_structural*(s_pos-(dist2*unit_vec));
				bend_force=bend_force+internal;
			}
			
			if(p.isFixed()==false)
			{
				Vec3f gravity_force=args->gravity*p.getMass();
				Vec3f accel = struc_force+shear_force+bend_force+gravity_force;
				
				accel=Vec3f(accel.x()/p.getMass(),accel.y()/p.getMass(),accel.z()/p.getMass());
				
				p.setAcceleration(accel);//*args->timestep);
				
				Vec3f Vi=p.getVelocity();
				Vec3f Vf=p.getVelocity()+ ( (p.getAcceleration()) * args->timestep);
				Vf=Vf*(1-damping);
				Vec3f Vif=Vi+Vf;
				
				Vec3f n_pos=(p.getPosition() + Vf*args->timestep);
				Vec3f pos=p.getPosition();
				
				//Adjust new position and new velocity accordingly
				p.setPosition( p.getPosition() + Vf*args->timestep );  //( ( Vec3f(Vif.x()/2,Vif.y()/2,Vif.z()/2) )*args->timestep) ); 	
				p.setVelocity(Vf);
			}
		}
	}
	
}


// ================================================================================
// ================================================================================
// some helper drawing functions
// ================================================================================

void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction) {
  Vec3f ab_o = b_o-a_o;
  Vec3f ab = b-a;
  float length_o = ab_o.Length(); // the original length
  float length = ab.Length();     // the current length
  if (length > (1+correction) * length_o ||
      length < (1-correction) * length_o) {
    // draw the spring in cyan if it's over-stretched
    glColor3f(0,1,1);
  } else {
    // otherwise draw it black
    glColor3f(0,0,0);
  }  
  glVertex3f(a.x(),a.y(),a.z());
  glVertex3f(b.x(),b.y(),b.z());
}

Vec3f getNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4)
{
	// compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  return normal; 

}

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4) {
  // compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
  
}

void DrawForce(const Vec3f &p, const Vec3f &f) {
  Vec3f tmp = p+f;
  glVertex3f(p.x(),p.y(),p.z());
  glVertex3f(tmp.x(),tmp.y(),tmp.z());
}

// ================================================================================
// ================================================================================

